/*
 * @Author: YuanQuan D
 * @Date: 2025-03-26 09:16:47
 * @LastEditTime: 2025-05-26 15:47:43
 * @FilePath: \F429_arm_control_slave\User_Code\Src\pneumatic_muscle_control.c
 */
#include "pneumatic_muscle_control.h"
#include "string.h"
#include "stdio.h"
#include "remote_control_decode.h"
#include <stdbool.h>
/*
加入占空比转电流板子
0-100 对应 0 - 25mA ----即10对应 2.5mA 1％对应0.25mA 4对应1mA
真正需要 4 - 20mA 即16 - 80 共64变化量 中间值 16 + 32 = 48 ---- 12mA
1Mpa 真正用到 0.2 - 0.6Mpa 中间值0.4Mpa ----- 4 - 20mA 对应 0 - 1Mpa 
                                       ----设置最大气压0.6Mpa输出： 16 * 60％ = 9.6 --> 13.6mA   ----->对应占空比输出13.6mA / 0.25mA = 54.4 ----> 2000 * 54.4％ = 1088
                                       ----设置最小气压0.2Mpa输出： 16 * 20％ = 3.2 --> 7.2mA    ----->对应占空比输出7.2mA / 0.25mA = 28.8 ----> 2000 * 28.8％ = 576
                                       ----设置初始气压0.4Mpa输出:  16 * 40％ = 6.4 --> 10.4mA    ----->对应占空比输出10.4mA / 0.25mA = 41.6 ----> 2000 * 41.6％ = 832

                                       ----设置放气电流4mA： 4mA / 0.25mA = 16 --- >2000 * 16％ = 320

0.01Mpa 即0.1bar变化量对应 16 * 1％ = 0.16mA  --- >0.16mA / 0.25mA = 0.64 ----> 2000 * 0.64％ = 20 * 0.64 = 12.8 ~ 13
*/


/*
PWM输出管脚
PC6 PC7 PC8 PI2 
PE5 PE6
PF7
*/

//腕部正弦函数 正负8 500个采样点
const float sin_value_product[500] = {0.000, 0.101, 0.201, 0.302, 0.402, 0.502, 0.603, 0.703, 0.803, 0.903, 1.003, 1.102, 1.202, 1.301, 1.400, 1.499, 1.598, 1.696, 1.794, 1.892, 1.990, 2.087, 2.184, 
    2.280, 2.376, 2.472, 2.568, 2.663, 2.757, 2.851, 2.945, 3.038, 3.131, 3.223, 3.315, 3.406, 3.497, 3.587, 3.677, 3.766, 3.854, 3.942, 4.029, 4.116, 4.201, 4.287, 4.371, 4.455, 4.538, 4.621, 
    4.702, 4.783, 4.863, 4.943, 5.022, 5.099, 5.176, 5.253, 5.328, 5.403, 5.476, 5.549, 5.621, 5.692, 5.762, 5.832, 5.900, 5.968, 6.034, 6.100, 6.164, 6.228, 6.290, 6.352, 6.413, 6.472, 6.531, 
    6.588, 6.645, 6.700, 6.755, 6.808, 6.860, 6.911, 6.961, 7.010, 7.058, 7.105, 7.151, 7.195, 7.239, 7.281, 7.322, 7.362, 7.401, 7.438, 7.475, 7.510, 7.544, 7.577, 7.608, 7.639, 7.668, 7.696, 
    7.723, 7.749, 7.773, 7.796, 7.818, 7.839, 7.858, 7.877, 7.893, 7.909, 7.924, 7.937, 7.949, 7.960, 7.969, 7.977, 7.984, 7.990, 7.994, 7.997, 7.999, 8.000, 7.999, 7.997, 7.994, 7.990, 7.984, 
    7.977, 7.969, 7.960, 7.949, 7.937, 7.924, 7.909, 7.893, 7.877, 7.858, 7.839, 7.818, 7.796, 7.773, 7.749, 7.723, 7.696, 7.668, 7.639, 7.608, 7.577, 7.544, 7.510, 7.475, 7.438, 7.401, 7.362, 
    7.322, 7.281, 7.239, 7.195, 7.151, 7.105, 7.058, 7.010, 6.961, 6.911, 6.860, 6.808, 6.755, 6.700, 6.645, 6.588, 6.531, 6.472, 6.413, 6.352, 6.290, 6.228, 6.164, 6.100, 6.034, 5.968, 5.900, 
    5.832, 5.762, 5.692, 5.621, 5.549, 5.476, 5.403, 5.328, 5.253, 5.176, 5.099, 5.022, 4.943, 4.863, 4.783, 4.702, 4.621, 4.538, 4.455, 4.371, 4.287, 4.201, 4.116, 4.029, 3.942, 3.854, 3.766, 
    3.677, 3.587, 3.497, 3.406, 3.315, 3.223, 3.131, 3.038, 2.945, 2.851, 2.757, 2.663, 2.568, 2.472, 2.376, 2.280, 2.184, 2.087, 1.990, 1.892, 1.794, 1.696, 1.598, 1.499, 1.400, 1.301, 1.202, 
    1.102, 1.003, 0.903, 0.803, 0.703, 0.603, 0.502, 0.402, 0.302, 0.201, 0.101, 0.000, -0.101, -0.201, -0.302, -0.402, -0.502, -0.603, -0.703, -0.803, -0.903, -1.003, -1.102, -1.202, -1.301, 
    -1.400, -1.499, -1.598, -1.696, -1.794, -1.892, -1.990, -2.087, -2.184, -2.280, -2.376, -2.472, -2.568, -2.663, -2.757, -2.851, -2.945, -3.038, -3.131, -3.223, -3.315, -3.406, -3.497, 
    -3.587, -3.677, -3.766, -3.854, -3.942, -4.029, -4.116, -4.201, -4.287, -4.371, -4.455, -4.538, -4.621, -4.702, -4.783, -4.863, -4.943, -5.022, -5.099, -5.176, -5.253, -5.328, -5.403, 
    -5.476, -5.549, -5.621, -5.692, -5.762, -5.832, -5.900, -5.968, -6.034, -6.100, -6.164, -6.228, -6.290, -6.352, -6.413, -6.472, -6.531, -6.588, -6.645, -6.700, -6.755, -6.808, -6.860, 
    -6.911, -6.961, -7.010, -7.058, -7.105, -7.151, -7.195, -7.239, -7.281, -7.322, -7.362, -7.401, -7.438, -7.475, -7.510, -7.544, -7.577, -7.608, -7.639, -7.668, -7.696, -7.723, -7.749, 
    -7.773, -7.796, -7.818, -7.839, -7.858, -7.877, -7.893, -7.909, -7.924, -7.937, -7.949, -7.960, -7.969, -7.977, -7.984, -7.990, -7.994, -7.997, -7.999, -8.000, -7.999, -7.997, -7.994, 
    -7.990, -7.984, -7.977, -7.969, -7.960, -7.949, -7.937, -7.924, -7.909, -7.893, -7.877, -7.858, -7.839, -7.818, -7.796, -7.773, -7.749, -7.723, -7.696, -7.668, -7.639, -7.608, -7.577, 
    -7.544, -7.510, -7.475, -7.438, -7.401, -7.362, -7.322, -7.281, -7.239, -7.195, -7.151, -7.105, -7.058, -7.010, -6.961, -6.911, -6.860, -6.808, -6.755, -6.700, -6.645, -6.588, -6.531, 
    -6.472, -6.413, -6.352, -6.290, -6.228, -6.164, -6.100, -6.034, -5.968, -5.900, -5.832, -5.762, -5.692, -5.621, -5.549, -5.476, -5.403, -5.328, -5.253, -5.176, -5.099, -5.022, -4.943, 
    -4.863, -4.783, -4.702, -4.621, -4.538, -4.455, -4.371, -4.287, -4.201, -4.116, -4.029, -3.942, -3.854, -3.766, -3.677, -3.587, -3.497, -3.406, -3.315, -3.223, -3.131, -3.038, -2.945, 
    -2.851, -2.757, -2.663, -2.568, -2.472, -2.376, -2.280, -2.184, -2.087, -1.990, -1.892, -1.794, -1.696, -1.598, -1.499, -1.400, -1.301, -1.202, -1.102, -1.003, -0.903, -0.803, -0.703, 
    -0.603, -0.502, -0.402, -0.302, -0.201, -0.101};

//肘部正弦函数 -10 --- -50°
const float elbow_sin_value_product[500] = {-10.000, -10.002, -10.006, -10.014, -10.025, -10.039, -10.057, -10.077, -10.101, -10.128, -10.158, -10.191, -10.227, -10.266, -10.309, -10.354, 
    -10.403, -10.455, -10.509, -10.567, -10.628, -10.692, -10.759, -10.830, -10.903, -10.979, -11.058, -11.140, -11.225, -11.313, -11.404, -11.498, -11.595, -11.695, -11.798, -11.903, -12.012,
     -12.123, -12.237, -12.354, -12.474, -12.596, -12.722, -12.849, -12.980, -13.113, -13.249, -13.388, -13.529, -13.673, -13.820, -13.969, -14.120, -14.274, -14.431, -14.590, -14.751, -14.915,
      -15.081, -15.250, -15.421, -15.594, -15.769, -15.947, -16.127, -16.309, -16.493, -16.680, -16.868, -17.059, -17.252, -17.446, -17.643, -17.841, -18.042, -18.244, -18.449, -18.655, -18.862,
       -19.072, -19.283, -19.497, -19.711, -19.928, -20.145, -20.365, -20.586, -20.808, -21.032, -21.258, -21.484, -21.712, -21.942, -22.173, -22.404, -22.638, -22.872, -23.107, -23.344, 
       -23.581, -23.820, -24.059, -24.300, -24.541, -24.783, -25.026, -25.270, -25.515, -25.760, -26.006, -26.252, -26.500, -26.747, -26.995, -27.244, -27.493, -27.743, -27.993, -28.243, 
       -28.493, -28.744, -28.995, -29.246, -29.497, -29.749, -30.000, -30.251, -30.503, -30.754, -31.005, -31.256, -31.507, -31.757, -32.007, -32.257, -32.507, -32.756, -33.005, -33.253, 
       -33.500, -33.748, -33.994, -34.240, -34.485, -34.730, -34.974, -35.217, -35.459, -35.700, -35.941, -36.180, -36.419, -36.656, -36.893, -37.128, -37.362, -37.596, -37.827, -38.058, 
       -38.288, -38.516, -38.742, -38.968, -39.192, -39.414, -39.635, -39.855, -40.072, -40.289, -40.503, -40.717, -40.928, -41.138, -41.345, -41.551, -41.756, -41.958, -42.159, -42.357, 
       -42.554, -42.748, -42.941, -43.132, -43.320, -43.507, -43.691, -43.873, -44.053, -44.231, -44.406, -44.579, -44.750, -44.919, -45.085, -45.249, -45.410, -45.569, -45.726, -45.880, 
       -46.031, -46.180, -46.327, -46.471, -46.612, -46.751, -46.887, -47.020, -47.151, -47.278, -47.404, -47.526, -47.646, -47.763, -47.877, -47.988, -48.097, -48.202, -48.305, -48.405, 
       -48.502, -48.596, -48.687, -48.775, -48.860, -48.942, -49.021, -49.097, -49.170, -49.241, -49.308, -49.372, -49.433, -49.491, -49.545, -49.597, -49.646, -49.691, -49.734, -49.773, 
       -49.809, -49.842, -49.872, -49.899, -49.923, -49.943, -49.961, -49.975, -49.986, -49.994, -49.998, -50.000, -49.998, -49.994, -49.986, -49.975, -49.961, -49.943, -49.923, -49.899, 
       -49.872, -49.842, -49.809, -49.773, -49.734, -49.691, -49.646, -49.597, -49.545, -49.491, -49.433, -49.372, -49.308, -49.241, -49.170, -49.097, -49.021, -48.942, -48.860, -48.775, 
       -48.687, -48.596, -48.502, -48.405, -48.305, -48.202, -48.097, -47.988, -47.877, -47.763, -47.646, -47.526, -47.404, -47.278, -47.151, -47.020, -46.887, -46.751, -46.612, -46.471, 
       -46.327, -46.180, -46.031, -45.880, -45.726, -45.569, -45.410, -45.249, -45.085, -44.919, -44.750, -44.579, -44.406, -44.231, -44.053, -43.873, -43.691, -43.507, -43.320, -43.132, 
       -42.941, -42.748, -42.554, -42.357, -42.159, -41.958, -41.756, -41.551, -41.345, -41.138, -40.928, -40.717, -40.503, -40.289, -40.072, -39.855, -39.635, -39.414, -39.192, -38.968, 
       -38.742, -38.516, -38.288, -38.058, -37.827, -37.596, -37.362, -37.128, -36.893, -36.656, -36.419, -36.180, -35.941, -35.700, -35.459, -35.217, -34.974, -34.730, -34.485, -34.240, 
       -33.994, -33.748, -33.500, -33.253, -33.005, -32.756, -32.507, -32.257, -32.007, -31.757, -31.507, -31.256, -31.005, -30.754, -30.503, -30.251, -30.000, -29.749, -29.497, -29.246, 
       -28.995, -28.744, -28.493, -28.243, -27.993, -27.743, -27.493, -27.244, -26.995, -26.747, -26.500, -26.252, -26.006, -25.760, -25.515, -25.270, -25.026, -24.783, -24.541, -24.300, 
       -24.059, -23.820, -23.581, -23.344, -23.107, -22.872, -22.638, -22.404, -22.173, -21.942, -21.712, -21.484, -21.258, -21.032, -20.808, -20.586, -20.365, -20.145, -19.928, -19.711, 
       -19.497, -19.283, -19.072, -18.862, -18.655, -18.449, -18.244, -18.042, -17.841, -17.643, -17.446, -17.252, -17.059, -16.868, -16.680, -16.493, -16.309, -16.127, -15.947, -15.769, 
       -15.594, -15.421, -15.250, -15.081, -14.915, -14.751, -14.590, -14.431, -14.274, -14.120, -13.969, -13.820, -13.673, -13.529, -13.388, -13.249, -13.113, -12.980, -12.849, -12.722,
       -12.596, -12.474, -12.354, -12.237, -12.123, -12.012, -11.903, -11.798, -11.695, -11.595, -11.498, -11.404, -11.313, -11.225, -11.140, -11.058, -10.979, -10.903, -10.830, -10.759, 
       -10.692, -10.628, -10.567, -10.509, -10.455, -10.403, -10.354, -10.309, -10.266, -10.227, -10.191, -10.158, -10.128, -10.101, -10.077, -10.057, -10.039, -10.025, -10.014, -10.006, 
       -10.002};

//肘部5000个采样点
const float elbow_sin_value_product_5000[5000] = {-10.000, -10.000, -10.000, -10.001, -10.001, -10.002, -10.002, -10.003, -10.004, -10.005, -10.006, -10.008, -10.009, -10.011, -10.012, 
    -10.014, -10.016, -10.018, -10.020, -10.023, -10.025, -10.028, -10.031, -10.033, -10.036, -10.039, -10.043, -10.046, -10.050, -10.053, -10.057, -10.061, -10.065, -10.069, -10.073, 
    -10.077, -10.082, -10.086, -10.091, -10.096, -10.101, -10.106, -10.111, -10.117, -10.122, -10.128, -10.134, -10.139, -10.145, -10.151, -10.158, -10.164, -10.171, -10.177, -10.184, 
    -10.191, -10.198, -10.205, -10.212, -10.219, -10.227, -10.235, -10.242, -10.250, -10.258, -10.266, -10.275, -10.283, -10.291, -10.300, -10.309, -10.318, -10.327, -10.336, -10.345, 
    -10.354, -10.364, -10.373, -10.383, -10.393, -10.403, -10.413, -10.423, -10.434, -10.444, -10.455, -10.465, -10.476, -10.487, -10.498, -10.509, -10.521, -10.532, -10.544, -10.556, 
    -10.567, -10.579, -10.591, -10.604, -10.616, -10.628, -10.641, -10.654, -10.666, -10.679, -10.692, -10.706, -10.719, -10.732, -10.746, -10.759, -10.773, -10.787, -10.801, -10.815, 
    -10.830, -10.844, -10.858, -10.873, -10.888, -10.903, -10.918, -10.933, -10.948, -10.963, -10.979, -10.994, -11.010, -11.026, -11.042, -11.058, -11.074, -11.091, -11.107, -11.124, 
    -11.140, -11.157, -11.174, -11.191, -11.208, -11.225, -11.243, -11.260, -11.278, -11.296, -11.313, -11.331, -11.349, -11.368, -11.386, -11.404, -11.423, -11.442, -11.461, -11.479, 
    -11.498, -11.518, -11.537, -11.556, -11.576, -11.595, -11.615, -11.635, -11.655, -11.675, -11.695, -11.715, -11.736, -11.756, -11.777, -11.798, -11.819, -11.840, -11.861, -11.882, 
    -11.903, -11.925, -11.946, -11.968, -11.990, -12.012, -12.034, -12.056, -12.078, -12.101, -12.123, -12.146, -12.168, -12.191, -12.214, -12.237, -12.260, -12.284, -12.307, -12.331, 
    -12.354, -12.378, -12.402, -12.426, -12.450, -12.474, -12.498, -12.523, -12.547, -12.572, -12.596, -12.621, -12.646, -12.671, -12.696, -12.722, -12.747, -12.772, -12.798, -12.824, 
    -12.849, -12.875, -12.901, -12.928, -12.954, -12.980, -13.007, -13.033, -13.060, -13.087, -13.113, -13.140, -13.168, -13.195, -13.222, -13.249, -13.277, -13.305, -13.332, -13.360, 
    -13.388, -13.416, -13.444, -13.473, -13.501, -13.529, -13.558, -13.587, -13.615, -13.644, -13.673, -13.702, -13.731, -13.761, -13.790, -13.820, -13.849, -13.879, -13.909, -13.939, 
    -13.969, -13.999, -14.029, -14.059, -14.090, -14.120, -14.151, -14.182, -14.212, -14.243, -14.274, -14.305, -14.337, -14.368, -14.399, -14.431, -14.462, -14.494, -14.526, -14.558, 
    -14.590, -14.622, -14.654, -14.686, -14.719, -14.751, -14.784, -14.816, -14.849, -14.882, -14.915, -14.948, -14.981, -15.014, -15.048, -15.081, -15.115, -15.148, -15.182, -15.216, 
    -15.250, -15.284, -15.318, -15.352, -15.386, -15.421, -15.455, -15.490, -15.524, -15.559, -15.594, -15.629, -15.664, -15.699, -15.734, -15.769, -15.805, -15.840, -15.876, -15.911, -15.947, -15.983, -16.019, -16.055, -16.091, -16.127, -16.163, -16.200, -16.236, -16.272, -16.309, -16.346, -16.383, -16.419, -16.456, -16.493, -16.530, -16.568, -16.605, -16.642, -16.680, -16.717, -16.755, -16.793, -16.830, -16.868, -16.906, -16.944, -16.982, -17.021, -17.059, -17.097, -17.136, -17.174, -17.213, -17.252, -17.290, -17.329, -17.368, -17.407, -17.446, -17.485, -17.525, -17.564, -17.603, -17.643, -17.682, -17.722, -17.762, -17.802, -17.841, -17.881, -17.921, -17.961, -18.002, -18.042, -18.082, -18.123, -18.163, -18.204, -18.244, -18.285, -18.326, -18.367, -18.408, -18.449, -18.490, -18.531, -18.572, -18.613, -18.655, -18.696, -18.738, -18.779, -18.821, -18.862, -18.904, -18.946, -18.988, -19.030, -19.072, -19.114, -19.156, -19.199, -19.241, -19.283, -19.326, -19.368, -19.411, -19.454, -19.497, -19.539, -19.582, -19.625, -19.668, -19.711, -19.754, -19.798, -19.841, -19.884, -19.928, -19.971, -20.015, -20.058, -20.102, -20.145, -20.189, -20.233, -20.277, -20.321, -20.365, -20.409, -20.453, -20.497, -20.542, -20.586, -20.630, -20.675, -20.719, -20.764, -20.808, -20.853, -20.898, -20.943, -20.987, -21.032, -21.077, -21.122, -21.167, -21.213, -21.258, -21.303, -21.348, -21.394, -21.439, -21.484, -21.530, -21.575, -21.621, -21.667, -21.712, -21.758, -21.804, -21.850, -21.896, -21.942, -21.988, -22.034, -22.080, -22.126, -22.173, -22.219, -22.265, -22.312, -22.358, -22.404, -22.451, -22.498, -22.544, -22.591, -22.638, -22.684, -22.731, -22.778, -22.825, -22.872, -22.919, -22.966, -23.013, -23.060, -23.107, -23.154, -23.202, -23.249, -23.296, -23.344, -23.391, -23.438, -23.486, -23.534, -23.581, -23.629, -23.676, -23.724, -23.772, -23.820, -23.867, -23.915, -23.963, -24.011, -24.059, -24.107, -24.155, -24.203, -24.251, -24.300, -24.348, -24.396, -24.444, -24.493, -24.541, -24.589, -24.638, -24.686, -24.735, -24.783, -24.832, -24.880, -24.929, -24.978, -25.026, -25.075, -25.124, -25.172, -25.221, -25.270, -25.319, -25.368, -25.417, -25.466, -25.515, -25.564, -25.613, -25.662, -25.711, -25.760, -25.809, -25.858, -25.907, -25.957, -26.006, -26.055, -26.104, -26.154, -26.203, -26.252, -26.302, -26.351, -26.401, -26.450, -26.500, -26.549, -26.599, -26.648, -26.698, -26.747, -26.797, -26.846, -26.896, -26.946, -26.995, -27.045, -27.095, -27.145, -27.194, -27.244, -27.294, -27.344, -27.394, -27.443, -27.493, -27.543, -27.593, -27.643, -27.693, -27.743, -27.793, -27.843, -27.893, -27.943, -27.993, -28.043, -28.093, -28.143, -28.193, -28.243, -28.293, -28.343, -28.393, -28.443, -28.493, -28.544, -28.594, -28.644, -28.694, -28.744, -28.794, -28.845, -28.895, -28.945, -28.995, -29.045, -29.096, -29.146, -29.196, -29.246, -29.296, -29.347, -29.397, -29.447, -29.497, -29.548, -29.598, -29.648, -29.698, -29.749, -29.799, -29.849, -29.899, -29.950, -30.000, -30.050, -30.101, -30.151, -30.201, -30.251, -30.302, -30.352, -30.402, -30.452, -30.503, -30.553, -30.603, -30.653, -30.704, -30.754, -30.804, -30.854, -30.904, -30.955, -31.005, -31.055, -31.105, -31.155, -31.206, -31.256, -31.306, -31.356, -31.406, -31.456, -31.507, -31.557, -31.607, -31.657, -31.707, -31.757, -31.807, -31.857, -31.907, -31.957, -32.007, -32.057, -32.107, -32.157, -32.207, -32.257, -32.307, -32.357, -32.407, -32.457, -32.507, -32.557, -32.606, -32.656, -32.706, -32.756, -32.806, -32.855, -32.905, -32.955, -33.005, -33.054, -33.104, -33.154, -33.203, -33.253, -33.302, -33.352, -33.401, -33.451, -33.500, -33.550, -33.599, -33.649, -33.698, -33.748, -33.797, -33.846, -33.896, -33.945, -33.994, -34.043, -34.093, -34.142, -34.191, -34.240, -34.289, -34.338, -34.387, -34.436, -34.485, -34.534, -34.583, -34.632, -34.681, -34.730, -34.779, -34.828, -34.876, -34.925, -34.974, -35.022, -35.071, -35.120, -35.168, -35.217, -35.265, -35.314, -35.362, -35.411, -35.459, -35.507, -35.556, -35.604, -35.652, -35.700, -35.749, -35.797, -35.845, -35.893, -35.941, -35.989, -36.037, -36.085, -36.133, -36.180, -36.228, -36.276, -36.324, -36.371, -36.419, -36.466, -36.514, -36.562, -36.609, -36.656, -36.704, -36.751, -36.798, -36.846, -36.893, -36.940, -36.987, -37.034, -37.081, -37.128, -37.175, -37.222, -37.269, -37.316, -37.362, -37.409, -37.456, -37.502, -37.549, -37.596, -37.642, -37.688, -37.735, -37.781, -37.827, -37.874, -37.920, -37.966, -38.012, -38.058, -38.104, -38.150, -38.196, -38.242, -38.288, -38.333, -38.379, -38.425, -38.470, -38.516, -38.561, -38.606, -38.652, -38.697, -38.742, -38.787, -38.833, -38.878, -38.923, -38.968, -39.013, -39.057, -39.102, -39.147, -39.192, -39.236, -39.281, -39.325, -39.370, -39.414, -39.458, -39.503, -39.547, -39.591, -39.635, -39.679, -39.723, -39.767, -39.811, -39.855, -39.898, -39.942, -39.985, -40.029, -40.072, -40.116, -40.159, -40.202, -40.246, -40.289, -40.332, -40.375, -40.418, -40.461, -40.503, -40.546, -40.589, -40.632, -40.674, -40.717, -40.759, -40.801, -40.844, -40.886, -40.928, -40.970, -41.012, -41.054, -41.096, -41.138, -41.179, -41.221, -41.262, -41.304, -41.345, -41.387, -41.428, -41.469, -41.510, -41.551, -41.592, -41.633, -41.674, -41.715, -41.756, -41.796, -41.837, -41.877, -41.918, -41.958, -41.998, -42.039, -42.079, -42.119, -42.159, -42.198, -42.238, -42.278, -42.318, -42.357, -42.397, -42.436, -42.475, -42.515, -42.554, -42.593, -42.632, -42.671, -42.710, -42.748, -42.787, -42.826, -42.864, -42.903, -42.941, -42.979, -43.018, -43.056, -43.094, -43.132, -43.170, -43.207, -43.245, -43.283, -43.320, -43.358, -43.395, -43.432, -43.470, -43.507, -43.544, -43.581, -43.617, -43.654, -43.691, -43.728, -43.764, -43.800, -43.837, -43.873, -43.909, -43.945, -43.981, -44.017, -44.053, -44.089, -44.124, -44.160, -44.195, -44.231, -44.266, -44.301, -44.336, -44.371, -44.406, -44.441, -44.476, -44.510, -44.545, -44.579, -44.614, -44.648, -44.682, -44.716, -44.750, -44.784, -44.818, -44.852, -44.885, -44.919, -44.952, -44.986, -45.019, -45.052, -45.085, -45.118, -45.151, -45.184, -45.216, -45.249, -45.281, -45.314, -45.346, -45.378, -45.410, -45.442, -45.474, -45.506, -45.538, -45.569, -45.601, -45.632, -45.663, -45.695, -45.726, -45.757, -45.788, -45.818, -45.849, -45.880, -45.910, -45.941, -45.971, -46.001, -46.031, -46.061, -46.091, -46.121, -46.151, -46.180, -46.210, -46.239, -46.269, -46.298, -46.327, -46.356, -46.385, -46.413, -46.442, -46.471, -46.499, -46.527, -46.556, -46.584, -46.612, -46.640, -46.668, -46.695, -46.723, -46.751, -46.778, -46.805, -46.832, -46.860, -46.887, -46.913, -46.940, -46.967, -46.993, -47.020, -47.046, -47.072, -47.099, -47.125, -47.151, -47.176, -47.202, -47.228, -47.253, -47.278, -47.304, -47.329, -47.354, -47.379, -47.404, -47.428, -47.453, -47.477, -47.502, -47.526, -47.550, -47.574, -47.598, -47.622, -47.646, -47.669, -47.693, -47.716, -47.740, -47.763, -47.786, -47.809, -47.832, -47.854, -47.877, -47.899, -47.922, -47.944, -47.966, -47.988, -48.010, -48.032, -48.054, -48.075, -48.097, -48.118, -48.139, -48.160, -48.181, -48.202, -48.223, -48.244, -48.264, -48.285, -48.305, -48.325, -48.345, -48.365, -48.385, -48.405, -48.424, -48.444, -48.463, -48.482, -48.502, -48.521, -48.539, -48.558, -48.577, -48.596, -48.614, -48.632, -48.651, -48.669, -48.687, -48.704, -48.722, -48.740, -48.757, -48.775, -48.792, -48.809, -48.826, -48.843, -48.860, -48.876, -48.893, -48.909, -48.926, -48.942, -48.958, -48.974, -48.990, -49.006, -49.021, -49.037, -49.052, -49.067, -49.082, -49.097, -49.112, -49.127, -49.142, -49.156, -49.170, -49.185, -49.199, -49.213, -49.227, -49.241, -49.254, -49.268, -49.281, -49.294, -49.308, -49.321, -49.334, -49.346, -49.359, -49.372, -49.384, -49.396, -49.409, -49.421, -49.433, -49.444, -49.456, -49.468, -49.479, -49.491, -49.502, -49.513, -49.524, -49.535, -49.545, -49.556, -49.566, -49.577, -49.587, -49.597, -49.607, -49.617, -49.627, -49.636, -49.646, -49.655, -49.664, -49.673, -49.682, -49.691, -49.700, -49.709, -49.717, -49.725, -49.734, -49.742, -49.750, -49.758, -49.765, -49.773, -49.781, -49.788, -49.795, -49.802, -49.809, -49.816, -49.823, -49.829, -49.836, -49.842, -49.849, -49.855, -49.861, -49.866, -49.872, -49.878, -49.883, -49.889, -49.894, -49.899, -49.904, -49.909, -49.914, -49.918, -49.923, -49.927, -49.931, -49.935, -49.939, -49.943, -49.947, -49.950, -49.954, -49.957, -49.961, -49.964, -49.967, -49.969, -49.972, -49.975, -49.977, -49.980, -49.982, -49.984, -49.986, -49.988, -49.989, -49.991, -49.992, -49.994, -49.995, -49.996, -49.997, -49.998, -49.998, -49.999, -49.999, -50.000, -50.000, -50.000, -50.000, -50.000, -49.999, -49.999, -49.998, -49.998, -49.997, -49.996, -49.995, -49.994, -49.992, -49.991, -49.989, -49.988, -49.986, -49.984, -49.982, -49.980, -49.977, -49.975, -49.972, -49.969, -49.967, -49.964, -49.961, -49.957, -49.954, -49.950, -49.947, -49.943, -49.939, -49.935, -49.931, -49.927, -49.923, -49.918, -49.914, -49.909, -49.904, -49.899, -49.894, -49.889, -49.883, -49.878, -49.872, -49.866, -49.861, -49.855, -49.849, -49.842, -49.836, -49.829, -49.823, -49.816, -49.809, -49.802, -49.795, -49.788, -49.781, -49.773, -49.765, -49.758, -49.750, -49.742, -49.734, -49.725, -49.717, -49.709, -49.700, -49.691, -49.682, -49.673, -49.664, -49.655, -49.646, -49.636, -49.627, -49.617, -49.607, -49.597, -49.587, -49.577, -49.566, -49.556, -49.545, -49.535, -49.524, -49.513, -49.502, -49.491, -49.479, -49.468, -49.456, -49.444, -49.433, -49.421, -49.409, -49.396, -49.384, -49.372, -49.359, -49.346, -49.334, -49.321, -49.308, -49.294, -49.281, -49.268, -49.254, -49.241, -49.227, -49.213, -49.199, -49.185, -49.170, -49.156, -49.142, -49.127, -49.112, -49.097, -49.082, -49.067, -49.052, -49.037, -49.021, -49.006, -48.990, -48.974, -48.958, -48.942, -48.926, -48.909, -48.893, -48.876, -48.860, -48.843, -48.826, -48.809, -48.792, -48.775, -48.757, -48.740, -48.722, -48.704, -48.687, -48.669, -48.651, -48.632, -48.614, -48.596, -48.577, -48.558, -48.539, -48.521, -48.502, -48.482, -48.463, -48.444, -48.424, -48.405, -48.385, -48.365, -48.345, -48.325, -48.305, -48.285, -48.264, -48.244, -48.223, -48.202, -48.181, -48.160, -48.139, -48.118, -48.097, -48.075, -48.054, -48.032, -48.010, -47.988, -47.966, -47.944, -47.922, -47.899, -47.877, -47.854, -47.832, -47.809, -47.786, -47.763, -47.740, -47.716, -47.693, -47.669, -47.646, -47.622, -47.598, -47.574, -47.550, -47.526, -47.502, -47.477, -47.453, -47.428, -47.404, -47.379, -47.354, -47.329, -47.304, -47.278, -47.253, -47.228, -47.202, -47.176, -47.151, -47.125, -47.099, -47.072, -47.046, -47.020, -46.993, -46.967, -46.940, -46.913, -46.887, -46.860, -46.832, -46.805, -46.778, -46.751, -46.723, -46.695, -46.668, -46.640, -46.612, -46.584, -46.556, -46.527, -46.499, -46.471, -46.442, -46.413, -46.385, -46.356, -46.327, -46.298, -46.269, -46.239, -46.210, -46.180, -46.151, -46.121, -46.091, -46.061, -46.031, -46.001, -45.971, -45.941, -45.910, -45.880, -45.849, -45.818, -45.788, -45.757, -45.726, -45.695, -45.663, -45.632, -45.601, -45.569, -45.538, -45.506, -45.474, -45.442, -45.410, -45.378, -45.346, -45.314, -45.281, -45.249, -45.216, -45.184, -45.151, -45.118, -45.085, -45.052, -45.019, -44.986, -44.952, -44.919, -44.885, -44.852, -44.818, -44.784, -44.750, -44.716, -44.682, -44.648, -44.614, -44.579, -44.545, -44.510, -44.476, -44.441, -44.406, -44.371, -44.336, -44.301, -44.266, -44.231, -44.195, -44.160, -44.124, -44.089, -44.053, -44.017, -43.981, -43.945, -43.909, -43.873, -43.837, -43.800, -43.764, -43.728, -43.691, -43.654, -43.617, -43.581, -43.544, -43.507, -43.470, -43.432, -43.395, -43.358, -43.320, -43.283, -43.245, -43.207, -43.170, -43.132, -43.094, -43.056, -43.018, -42.979, -42.941, -42.903, -42.864, -42.826, -42.787, -42.748, -42.710, -42.671, -42.632, -42.593, -42.554, -42.515, -42.475, -42.436, -42.397, -42.357, -42.318, -42.278, -42.238, -42.198, -42.159, -42.119, -42.079, -42.039, -41.998, -41.958, -41.918, -41.877, -41.837, -41.796, -41.756, -41.715, -41.674, -41.633, -41.592, -41.551, -41.510, -41.469, -41.428, -41.387, -41.345, -41.304, -41.262, -41.221, -41.179, -41.138, -41.096, -41.054, -41.012, -40.970, -40.928, -40.886, -40.844, -40.801, -40.759, -40.717, -40.674, -40.632, -40.589, -40.546, -40.503, -40.461, -40.418, -40.375, -40.332, -40.289, -40.246, -40.202, -40.159, -40.116, -40.072, -40.029, -39.985, -39.942, -39.898, -39.855, -39.811, -39.767, -39.723, -39.679, -39.635, -39.591, -39.547, -39.503, -39.458, -39.414, -39.370, -39.325, -39.281, -39.236, -39.192, -39.147, -39.102, -39.057, -39.013, -38.968, -38.923, -38.878, -38.833, -38.787, -38.742, -38.697, -38.652, -38.606, -38.561, -38.516, -38.470, -38.425, -38.379, -38.333, -38.288, -38.242, -38.196, -38.150, -38.104, -38.058, -38.012, -37.966, -37.920, -37.874, -37.827, -37.781, -37.735, -37.688, -37.642, -37.596, -37.549, -37.502, -37.456, -37.409, -37.362, -37.316, -37.269, -37.222, -37.175, -37.128, -37.081, -37.034, -36.987, -36.940, -36.893, -36.846, -36.798, -36.751, -36.704, -36.656, -36.609, -36.562, -36.514, -36.466, -36.419, -36.371, -36.324, -36.276, -36.228, -36.180, -36.133, -36.085, -36.037, -35.989, -35.941, -35.893, -35.845, -35.797, -35.749, -35.700, -35.652, -35.604, -35.556, -35.507, -35.459, -35.411, -35.362, -35.314, -35.265, -35.217, -35.168, -35.120, -35.071, -35.022, -34.974, -34.925, -34.876, -34.828, -34.779, -34.730, -34.681, -34.632, -34.583, -34.534, -34.485, -34.436, -34.387, -34.338, -34.289, -34.240, -34.191, -34.142, -34.093, -34.043, -33.994, -33.945, -33.896, -33.846, -33.797, -33.748, -33.698, -33.649, -33.599, -33.550, -33.500, -33.451, -33.401, -33.352, -33.302, -33.253, -33.203, -33.154, -33.104, -33.054, -33.005, -32.955, -32.905, -32.855, -32.806, -32.756, -32.706, -32.656, -32.606, -32.557, -32.507, -32.457, -32.407, -32.357, -32.307, -32.257, -32.207, -32.157, -32.107, -32.057, -32.007, -31.957, -31.907, -31.857, -31.807, -31.757, -31.707, -31.657, -31.607, -31.557, -31.507, -31.456, -31.406, -31.356, -31.306, -31.256, -31.206, -31.155, -31.105, -31.055, -31.005, -30.955, -30.904, -30.854, -30.804, -30.754, -30.704, -30.653, -30.603, -30.553, -30.503, -30.452, -30.402, -30.352, -30.302, -30.251, -30.201, -30.151, -30.101, -30.050, -30.000, -29.950, -29.899, -29.849, -29.799, -29.749, -29.698, -29.648, -29.598, -29.548, -29.497, -29.447, -29.397, -29.347, -29.296, -29.246, -29.196, -29.146, -29.096, -29.045, -28.995, -28.945, -28.895, -28.845, -28.794, -28.744, -28.694, -28.644, -28.594, -28.544, -28.493, -28.443, -28.393, -28.343, -28.293, -28.243, -28.193, -28.143, -28.093, -28.043, -27.993, -27.943, -27.893, -27.843, -27.793, -27.743, -27.693, -27.643, -27.593, -27.543, -27.493, -27.443, -27.394, -27.344, -27.294, -27.244, -27.194, -27.145, -27.095, -27.045, -26.995, -26.946, -26.896, -26.846, -26.797, -26.747, -26.698, -26.648, -26.599, -26.549, -26.500, -26.450, -26.401, -26.351, -26.302, -26.252, -26.203, -26.154, -26.104, -26.055, -26.006, -25.957, -25.907, -25.858, -25.809, -25.760, -25.711, -25.662, -25.613, -25.564, -25.515, -25.466, -25.417, -25.368, -25.319, -25.270, -25.221, -25.172, -25.124, -25.075, -25.026, -24.978, -24.929, -24.880, -24.832, -24.783, -24.735, -24.686, -24.638, -24.589, -24.541, -24.493, -24.444, -24.396, -24.348, -24.300, -24.251, -24.203, -24.155, -24.107, -24.059, -24.011, -23.963, -23.915, -23.867, -23.820, -23.772, -23.724, -23.676, -23.629, -23.581, -23.534, -23.486, -23.438, -23.391, -23.344, -23.296, -23.249, -23.202, -23.154, -23.107, -23.060, -23.013, -22.966, -22.919, -22.872, -22.825, -22.778, -22.731, -22.684, -22.638, -22.591, -22.544, -22.498, -22.451, -22.404, -22.358, -22.312, -22.265, -22.219, -22.173, -22.126, -22.080, -22.034, -21.988, -21.942, -21.896, -21.850, -21.804, -21.758, -21.712, -21.667, -21.621, -21.575, -21.530, -21.484, -21.439, -21.394, -21.348, -21.303, -21.258, -21.213, -21.167, -21.122, -21.077, -21.032, -20.987, -20.943, -20.898, -20.853, -20.808, -20.764, -20.719, -20.675, -20.630, -20.586, -20.542, -20.497, -20.453, -20.409, -20.365, -20.321, -20.277, -20.233, -20.189, -20.145, -20.102, -20.058, -20.015, -19.971, -19.928, -19.884, -19.841, -19.798, -19.754, -19.711, -19.668, -19.625, -19.582, -19.539, -19.497, -19.454, -19.411, -19.368, -19.326, -19.283, -19.241, -19.199, -19.156, -19.114, -19.072, -19.030, -18.988, -18.946, -18.904, -18.862, -18.821, -18.779, -18.738, -18.696, -18.655, -18.613, -18.572, -18.531, -18.490, -18.449, -18.408, -18.367, -18.326, -18.285, -18.244, -18.204, -18.163, -18.123, -18.082, -18.042, -18.002, -17.961, -17.921, -17.881, -17.841, -17.802, -17.762, -17.722, -17.682, -17.643, -17.603, -17.564, -17.525, -17.485, -17.446, -17.407, -17.368, -17.329, -17.290, -17.252, -17.213, -17.174, -17.136, -17.097, -17.059, -17.021, -16.982, -16.944, -16.906, -16.868, -16.830, -16.793, -16.755, -16.717, -16.680, -16.642, -16.605, -16.568, -16.530, -16.493, -16.456, -16.419, -16.383, -16.346, -16.309, -16.272, -16.236, -16.200, -16.163, -16.127, -16.091, -16.055, -16.019, -15.983, -15.947, -15.911, -15.876, -15.840, -15.805, -15.769, -15.734, -15.699, -15.664, -15.629, -15.594, -15.559, -15.524, -15.490, -15.455, -15.421, -15.386, -15.352, -15.318, -15.284, -15.250, -15.216, -15.182, -15.148, -15.115, -15.081, -15.048, -15.014, -14.981, -14.948, -14.915, -14.882, -14.849, -14.816, -14.784, -14.751, -14.719, -14.686, -14.654, -14.622, -14.590, -14.558, -14.526, -14.494, -14.462, -14.431, -14.399, -14.368, -14.337, -14.305, -14.274, -14.243, -14.212, -14.182, -14.151, -14.120, -14.090, -14.059, -14.029, -13.999, -13.969, -13.939, -13.909, -13.879, -13.849, -13.820, -13.790, -13.761, -13.731, -13.702, -13.673, -13.644, -13.615, -13.587, -13.558, -13.529, -13.501, -13.473, -13.444, -13.416, -13.388, -13.360, -13.332, -13.305, -13.277, -13.249, -13.222, -13.195, -13.168, -13.140, -13.113, -13.087, -13.060, -13.033, -13.007, -12.980, -12.954, -12.928, -12.901, -12.875, -12.849, -12.824, -12.798, -12.772, -12.747, -12.722, -12.696, -12.671, -12.646, -12.621, -12.596, -12.572, -12.547, -12.523, -12.498, -12.474, -12.450, -12.426, -12.402, -12.378, -12.354, -12.331, -12.307, -12.284, -12.260, -12.237, -12.214, -12.191, -12.168, -12.146, -12.123, -12.101, -12.078, -12.056, -12.034, -12.012, -11.990, -11.968, -11.946, -11.925, -11.903, -11.882, -11.861, -11.840, -11.819, -11.798, -11.777, -11.756, -11.736, -11.715, -11.695, -11.675, -11.655, -11.635, -11.615, -11.595, -11.576, -11.556, -11.537, -11.518, -11.498, -11.479, -11.461, -11.442, -11.423, -11.404, -11.386, -11.368, -11.349, -11.331, -11.313, -11.296, -11.278, -11.260, -11.243, -11.225, -11.208, -11.191, -11.174, -11.157, -11.140, -11.124, -11.107, -11.091, -11.074, -11.058, -11.042, -11.026, -11.010, -10.994, -10.979, -10.963, -10.948, -10.933, -10.918, -10.903, -10.888, -10.873, -10.858, -10.844, -10.830, -10.815, -10.801, -10.787, -10.773, -10.759, -10.746, -10.732, -10.719, -10.706, -10.692, -10.679, -10.666, -10.654, -10.641, -10.628, -10.616, -10.604, -10.591, -10.579, -10.567, -10.556, -10.544, -10.532, -10.521, -10.509, -10.498, -10.487, -10.476, -10.465, -10.455, -10.444, -10.434, -10.423, -10.413, -10.403, -10.393, -10.383, -10.373, -10.364, -10.354, -10.345, -10.336, -10.327, -10.318, -10.309, -10.300, -10.291, -10.283, -10.275, -10.266, -10.258, -10.250, -10.242, -10.235, -10.227, -10.219, -10.212, -10.205, -10.198, -10.191, -10.184, -10.177, -10.171, -10.164, -10.158, -10.151, -10.145, -10.139, -10.134, -10.128, -10.122, -10.117, -10.111, -10.106, -10.101, -10.096, -10.091, -10.086, -10.082, -10.077, -10.073, -10.069, -10.065, -10.061, -10.057, -10.053, -10.050, -10.046, -10.043, -10.039, -10.036, -10.033, -10.031, -10.028, -10.025, -10.023, -10.020, -10.018, -10.016, -10.014, -10.012, -10.011, -10.009, -10.008, -10.006, -10.005, -10.004, -10.003, -10.002, -10.002, -10.001, -10.001, -10.000, -10.000};

#define PPV1_U_input(trachea)  __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_1, trachea);
#define PPV2_U_input(trachea)  __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_2, trachea);
#define PPV3_U_input(trachea)  __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_3, trachea);
#define PPV4_U_input(trachea)  __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_4, trachea);
#define PPV5_U_input(trachea)  __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_1, trachea);
#define PPV6_U_input(trachea)  __HAL_TIM_SetCompare(&htim9, TIM_CHANNEL_2, trachea);
#define PPV7_U_input(trachea)  __HAL_TIM_SetCompare(&htim11, TIM_CHANNEL_1, trachea);

extern osEventFlagsId_t ToMaster_eventHandle;
uint8_t send_master_data_pack[200];
uint16_t input_ppv_ccr_1 = 0;
uint16_t input_ppv_ccr_2 = 0;
uint16_t input_ppv_ccr_3 = 0;
uint16_t input_ppv_ccr_4 = 0;
uint16_t input_ppv_ccr_5 = 0;
uint16_t input_ppv_ccr_6 = 0;
uint16_t input_ppv_ccr_7 = 0;

uint32_t ADC_ConvertedValue;
uint16_t ADC_VALUE[CHANNELS];
float ADC_Real_Value[CHANNELS];
extern ADC_HandleTypeDef hadc1;
extern TIM_HandleTypeDef htim8;
extern TIM_HandleTypeDef htim9;
extern TIM_HandleTypeDef htim11;
extern float fAngle[3];
extern float roll,pitch,yaw;
extern uint8_t tendon_Ctr_Flag;
extern uint8_t remote_ctr_mode;
extern RC_channelTypedef RC_channel_3;
extern RC_channelTypedef RC_channel_7;

uint8_t wrist_trachea_increment_dir = 0;
uint8_t elbow_trachea_increment_dir = 0;

int16_t wrist_trachea = trachea_init;
int16_t elbow_trachea_ = trachea_init;  // 34 变 47  ---- 3的电流反馈信号有误 难以形成闭环

uint8_t axiz_conversion_count = 0;

float real_r_voltage[pp_num + 1] = {0};
float real_r_trachea[pp_num + 1] = {0};
float real_r_trachea_storage[pp_num + 1][slide_window] = {0};
float real_r_trachea_storage_sum[pp_num + 1] = 0;
float real_r_trachea_slide[pp_num + 1] = {0};
float real_r_trachea_rc_filter_old[pp_num + 1] = {0};
float real_r_trachea_rc_filter_new[pp_num + 1] = {0};
float rc_a = 0.2;

//加入的腕部角度PID参数
// 单PID
// float PAM_x_kp = 0.38;
// float PAM_x_kd = 16.8;

// 串级PID
// float PAM_x_kp = 0.098;
// float PAM_x_kd = 0.00085;

// 仅剩内环PID --- 他妈的 放气快的一比
float PAM_x_kp = 0.0;
float PAM_x_kd = 0.0;
float angle_x_error = 0.0;
float last_angle_x_error = 0;
float angle_x = 0;
float last_angle_x = 0;
int32_t PAM_x_PWM_Output = 0;
double PWM_x_Output_real = 0;
float target_angle_x = 3.0;

//加入的腕部气压PID参数
pressure_pid wrist_PAM1;
pressure_pid wrist_PAM2;
pressure_pid wrist_PAM3;
pressure_pid wrist_PAM4;
pressure_pid wrist_PAM5;
pressure_pid wrist_PAM6;
pressure_pid wrist_PAM7;

//加入的肘部角度PID参数
float elbow_kp = 0.3;
float elbow_kd = 1.8;
float elbow_angle_error = 0;
float elbow_last_angle_error = 0;
float elbow_angle = 0;
float elbow_last_angle = 0;
int32_t elbow_PWM_Output = 0;
double elbow_PWM_Output_real = 0;
float elbow_target_angle = -(40); //最大-53 最小-2

uint8_t switch_PAM_outzero_flag = 1;

float wrist_trachea_diff = 0;

void expect_angle_change(void)
{
    static uint16_t i = 0;
    target_angle_x = 0;
    // target_angle_x = sin_value_product[i];   //腕部
    i ++;
    if(i >= 499) i = 0;

    // elbow_target_angle = elbow_sin_value_product_5000[i]; //肘部
    // i ++;
    // if(i >= 4999) i = 0;
}

//实际上： 15 26 34

void ppv_init(void)
{
    HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_4);
    HAL_TIM_PWM_Start(&htim9,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim9,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim11,TIM_CHANNEL_1);
    
    PPV1_U_input(trachea_init); 
    PPV2_U_input(trachea_init); 
    PPV3_U_input(trachea_init); 
    PPV4_U_input(trachea_init); 
    PPV5_U_input(trachea_init); 
    PPV6_U_input(trachea_init); 
    PPV7_U_input(trachea_init); 
}

void PAM_PID_Init(void)
{
    wrist_PAM1.kp = 0.005;
    wrist_PAM1.kd = 0.0001;
    wrist_PAM1.ki = 0.0;
    wrist_PAM1.error_L = wrist_PAM1.error_N = wrist_PAM1.output_real = 0;

    wrist_PAM5.kp = 0.005;
    wrist_PAM5.kd = 0.0001;
    wrist_PAM5.ki = 0.0;
    wrist_PAM5.error_L = wrist_PAM5.error_N = wrist_PAM5.output_real = 0;
}

void adc_init(void)
{
    HAL_ADC_Start_DMA(&hadc1, (uint32_t *)ADC_VALUE, CHANNELS);
}

void adc_stop(void)
{
    HAL_ADC_Stop_DMA(&hadc1);
}


// 参数配置
#define FILTER_WINDOW_SIZE  20   // 滑动窗口大小（建议32~64）
#define ADC_CHANNELS       7     // ADC通道数（实际使用6个，通道3跳过）

// 定义循环队列结构（存储历史ADC数据）
typedef struct {
    uint16_t buffer[ADC_CHANNELS][FILTER_WINDOW_SIZE]; // 通道×窗口大小
    uint8_t index;                                     // 当前写入位置
    bool is_filled;                                    // 队列是否已填满
} ADCFilterQueue;

ADCFilterQueue adc_queue = {0}; // 全局队列实例

// 更新队列并计算滤波后的值

void PPV_Cal(void)
{
    //实际反馈值 先低位后高位
    // send_master_data_pack[index ++] = ADC_VALUE[0] & 0xff;
    // send_master_data_pack[index ++] = (ADC_VALUE[0] >> 8) & 0xff;
    // send_master_data_pack[index ++] = ADC_VALUE[1] & 0xff;
    // send_master_data_pack[index ++] = (ADC_VALUE[1] >> 8) & 0xff;
    // send_master_data_pack[index ++] = ADC_VALUE[2] & 0xff;
    // send_master_data_pack[index ++] = (ADC_VALUE[2] >> 8) & 0xff;
    // send_master_data_pack[index ++] = ADC_VALUE[3] & 0xff;
    // send_master_data_pack[index ++] = (ADC_VALUE[3] >> 8) & 0xff;
    // send_master_data_pack[index ++] = ADC_VALUE[4] & 0xff;
    // send_master_data_pack[index ++] = (ADC_VALUE[4] >> 8) & 0xff;
    // send_master_data_pack[index ++] = ADC_VALUE[5] & 0xff;
    // send_master_data_pack[index ++] = (ADC_VALUE[5] >> 8) & 0xff;
    // send_master_data_pack[index ++] = ADC_VALUE[6] & 0xff;
    // send_master_data_pack[index ++] = (ADC_VALUE[6] >> 8) & 0xff;

    // real_r_trachea[1] = ADC_VALUE[0] / 409.6;
    // real_r_trachea[2] = ADC_VALUE[1] / 409.6;
    // real_r_trachea[3] = 0;
    // real_r_trachea[4] = ADC_VALUE[3] / 409.6;
    // real_r_trachea[5] = ADC_VALUE[4] / 409.6;
    // real_r_trachea[6] = ADC_VALUE[5] / 409.6;
    // real_r_trachea[7] = ADC_VALUE[6] / 409.6;

    // 1. 将最新ADC数据写入队列
    for (int ch = 0; ch < ADC_CHANNELS; ch++) {
        adc_queue.buffer[ch][adc_queue.index] = ADC_VALUE[ch];
    }
    adc_queue.index = (adc_queue.index + 1) % FILTER_WINDOW_SIZE; // 循环指针
    if (!adc_queue.is_filled && adc_queue.index == 0) {
        adc_queue.is_filled = true; // 标记队列已填满
    }

    // 2. 对每个通道计算滤波值（跳过通道3）
    for (int ch = 0; ch < ADC_CHANNELS; ch++) {
        if (ch == 2) {
            real_r_trachea[3] = 0; // 通道3固定为0
            continue;
        }

        // 极值处理：找出窗口内的最大值和最小值
        uint16_t min_val = 4095, max_val = 0;
        uint32_t sum = 0;
        uint8_t count = adc_queue.is_filled ? FILTER_WINDOW_SIZE : adc_queue.index;

        for (int i = 0; i < count; i++) {
            uint16_t val = adc_queue.buffer[ch][i];
            if (val < min_val) min_val = val;
            if (val > max_val) max_val = val;
            sum += val;
        }

        // 剔除最大最小值后求均值
        if (count >= 3) { // 至少3个数据才能剔除极值
            sum -= (min_val + max_val);
            count -= 2;
        }
        uint16_t adc_avg = (count > 0) ? (sum / count) : 0;

        // 转换为实际值（如电压、气压等）
        real_r_trachea[ch + 1] = adc_avg / 409.6f; // 注意：ch+1对应你的数组下标

        real_r_trachea_rc_filter_new[ch + 1] = real_r_trachea[ch + 1];
    }

    // 用实际值作为观测量 偏差太大 0.3bar左右 而且部分区域不线性变化 例如 100的输入 实际上是80 而200的输入 实际是220
    // real_r_trachea_rc_filter_new[1] = (float)((0.25 * input_ppv_ccr_1 / ARR_COUNT * 100  - 4.0) / 16.0 * 10.0);
    // real_r_trachea_rc_filter_new[2] = (float)((0.25 * input_ppv_ccr_2 / ARR_COUNT * 100  - 4.0) / 16.0 * 10.0);
    // real_r_trachea_rc_filter_new[3] = 0;
    // real_r_trachea_rc_filter_new[4] = (float)((0.25 * input_ppv_ccr_4 / ARR_COUNT * 100  - 4.0) / 16.0 * 10.0);
    // real_r_trachea_rc_filter_new[5] = (float)((0.25 * input_ppv_ccr_5 / ARR_COUNT * 100  - 4.0) / 16.0 * 10.0);
    // real_r_trachea_rc_filter_new[6] = (float)((0.25 * input_ppv_ccr_6 / ARR_COUNT * 100  - 4.0) / 16.0 * 10.0);
    // real_r_trachea_rc_filter_new[7] = (float)((0.25 * input_ppv_ccr_7 / ARR_COUNT * 100  - 4.0) / 16.0 * 10.0);
    // for(uint8_t m = 0; m < 8; m ++)
    //     real_r_trachea_storage_sum[m] = 0;

    // for(uint8_t i = 1; i < 8; i ++) //七个比例阀
    // {
    //     //滑动滤波
    //     for(int8_t j = (slide_window - 2); j >= 0; j --) 
    //     {
    //         real_r_trachea_storage[i][j + 1] = real_r_trachea_storage[i][j];
    //         if(j == 0)
    //         {
    //             real_r_trachea_storage[i][j] = real_r_trachea[i];
    //         }
    //     }
        
    //     for(uint8_t k = 0; k < slide_window; k ++)
    //     {
    //         real_r_trachea_storage_sum[i] += real_r_trachea_storage[i][k];
    //     }
    //     real_r_trachea_slide[i] = real_r_trachea_storage_sum[i] / slide_window;

    //     //低通滤波
    //     real_r_trachea_rc_filter_new[i] = rc_a * real_r_trachea_slide[i] + (1 - rc_a) * real_r_trachea_rc_filter_old[i];
    //     real_r_trachea_rc_filter_old[i] = real_r_trachea_rc_filter_new[i];

    //     //单rc低通滤波
    //     // real_r_trachea_rc_filter_new[i] = rc_a * real_r_trachea[i] + (1 - rc_a) * real_r_trachea_rc_filter_old[i];
    //     // real_r_trachea_rc_filter_old[i] = real_r_trachea_rc_filter_new[i];
    // } 
}

void wrist_x_motion(void)
{
    if(!wrist_trachea_increment_dir)
    {
        wrist_trachea += 5;
    }
    else
    {
        wrist_trachea -= 5;
    }
    //15增减
    PPV1_U_input(wrist_trachea); 
    PPV5_U_input(abs(trachea_range - wrist_trachea)); 
    input_ppv_ccr_1 = wrist_trachea;
    input_ppv_ccr_5 = abs(trachea_range - wrist_trachea);  

    //26固定
    PPV2_U_input(trachea_init); 
    PPV6_U_input(trachea_init); 
    input_ppv_ccr_2 = trachea_init;
    input_ppv_ccr_6 = trachea_init;
}

void wrist_y_motion(void)
{
    if(!wrist_trachea_increment_dir)
    {
        wrist_trachea += 1;
    }
    else
    {
        wrist_trachea -= 1;
    }
    //26一加一减
    PPV2_U_input(wrist_trachea); 
    PPV6_U_input(abs(trachea_range - wrist_trachea));  
    input_ppv_ccr_2 = wrist_trachea;
    input_ppv_ccr_6 = abs(trachea_range - wrist_trachea);

    //15固定
    PPV1_U_input(trachea_init); 
    PPV5_U_input(trachea_init); 
    input_ppv_ccr_1 = trachea_init;
    input_ppv_ccr_5 = trachea_init;
}

void ppv_wrist_control(void)
{
     //腕部 15 26 x轴依次
     //肘部 12 56 y轴依次
    if(tendon_Ctr_Flag)
    {
        // //判断是否满足10s 满足则执行以下
        //先复位
        // PPV1_U_input(trachea_init); 
        // PPV5_U_input(trachea_init); 
        // PPV2_U_input(trachea_init); 
        // PPV6_U_input(trachea_init); 
        // //延时2s稳定
        // osDelay(2000);
        // if(wrist_trachea < wrist_trachea_max_limit && wrist_trachea <= wrist_trachea_min_limit)
        // {
        //     wrist_trachea_increment_dir = 0;
        //     axiz_conversion_count ++;
        // }
        // else if(wrist_trachea >= wrist_trachea_max_limit)
        // {
        //     wrist_trachea_increment_dir = 1;
        //     axiz_conversion_count ++;
        // }

        // // wrist_x_motion();
        // // wrist_y_motion();
        // if(axiz_conversion_count <= 100)
        // {
        //     wrist_x_motion();
        // }
        // else if(axiz_conversion_count > 100)
        // {
        //     wrist_y_motion();
        //     if(axiz_conversion_count >= 200) axiz_conversion_count = 0;
        // }

        // PPV1_U_input(wrist_trachea_max_limit); 
        // PPV5_U_input(wrist_trachea_max_limit); 

        // PPV2_U_input(wrist_trachea_max_limit); 
        // PPV6_U_input(wrist_trachea_max_limit); 
        // input_ppv_ccr_1 = trachea_init;
        // input_ppv_ccr_5 = trachea_init;
        // input_ppv_ccr_2 = trachea_init;
        // input_ppv_ccr_6 = trachea_init;


        // //腕部PID控制 
        // last_angle_x_error = angle_x_error;
        // angle_x_error = target_angle_x - angle_x;
        // PWM_x_Output_real += PAM_x_kp * angle_x_error + PAM_x_kd * (angle_x_error - last_angle_x_error);
        // PAM_x_PWM_Output = (int16_t)PWM_x_Output_real;
        // wrist_trachea = trachea_init + PAM_x_PWM_Output;
        // if(wrist_trachea >= wrist_trachea_max_limit) wrist_trachea = wrist_trachea_max_limit;
        // else if(wrist_trachea <= wrist_trachea_min_limit) wrist_trachea = wrist_trachea_min_limit;

        // // //15增减 
        // PPV1_U_input(wrist_trachea - trachea_1_6_bias); 
        // PPV5_U_input(abs(trachea_range - wrist_trachea)); 
        // input_ppv_ccr_1 = wrist_trachea;
        // input_ppv_ccr_5 = abs(trachea_range - wrist_trachea);  
        // PPV2_U_input(trachea_init); 
        // PPV6_U_input(trachea_init - trachea_1_6_bias); 
        // input_ppv_ccr_2 = trachea_init;
        // input_ppv_ccr_6 = trachea_init;

        //串级PID控制
        //角度外环
        // last_angle_x_error = angle_x_error;
        // angle_x_error = target_angle_x - angle_x;
        // PWM_x_Output_real += PAM_x_kp * angle_x_error + PAM_x_kd * (angle_x_error - last_angle_x_error);
        // PAM_x_PWM_Output = (int32_t)PWM_x_Output_real;
        // wrist_trachea = trachea_init + PAM_x_PWM_Output;
        // if(wrist_trachea >= wrist_trachea_max_limit) wrist_trachea = wrist_trachea_max_limit;
        // else if(wrist_trachea <= wrist_trachea_min_limit) wrist_trachea = wrist_trachea_min_limit;
        // wrist_trachea = trachea_init;

        wrist_trachea = (trachea_init_bar - wrist_trachea_diff / 2) * 128 + 320;

        //气压内环  肌腱1
        wrist_PAM1.error_L = wrist_PAM1.error_N;
        wrist_PAM1.error_N = wrist_trachea - (real_r_trachea_rc_filter_new[1] * 128 + 320);
        wrist_PAM1.output_real += wrist_PAM1.kp * wrist_PAM1.error_N + wrist_PAM1.kd * (wrist_PAM1.error_N - wrist_PAM1.error_L); 
        wrist_PAM1.output = (int32_t)wrist_PAM1.output_real;
        if(wrist_PAM1.output >= wrist_trachea_max_limit) wrist_PAM1.output = wrist_trachea_max_limit;
        else if(wrist_PAM1.output <= wrist_trachea_min_limit) wrist_PAM1.output = wrist_trachea_min_limit;

        //气压内环 肌腱5
        wrist_PAM5.error_L = wrist_PAM5.error_N;
        wrist_PAM5.error_N = trachea_range - wrist_trachea - (real_r_trachea_rc_filter_new[5] * 128 + 320);
        wrist_PAM5.output_real += wrist_PAM5.kp * wrist_PAM5.error_N + wrist_PAM5.kd * (wrist_PAM5.error_N - wrist_PAM5.error_L);
        wrist_PAM5.output = (int32_t)wrist_PAM5.output_real;
        if(wrist_PAM5.output >= wrist_trachea_max_limit) wrist_PAM5.output = wrist_trachea_max_limit;
        else if(wrist_PAM5.output <= wrist_trachea_min_limit) wrist_PAM5.output = wrist_trachea_min_limit;

        //赋值
        PPV1_U_input(wrist_PAM1.output - trachea_1_6_bias); 
        PPV5_U_input(wrist_PAM5.output); 
        input_ppv_ccr_1 = wrist_PAM1.output;
        input_ppv_ccr_5 = wrist_PAM5.output;  
        PPV2_U_input(trachea_init); 
        PPV6_U_input(trachea_init - trachea_1_6_bias); 
        input_ppv_ccr_2 = trachea_init;
        input_ppv_ccr_6 = trachea_init;
    }

    else if(remote_ctr_mode == 1)
    {
        PPV1_U_input(RC_channel_3.real_Ctr_ang - trachea_1_6_bias); 
        PPV5_U_input(abs(trachea_range - 200 - RC_channel_3.real_Ctr_ang)); 
        input_ppv_ccr_1 = RC_channel_3.real_Ctr_ang;
        input_ppv_ccr_5 = abs(trachea_range - 200 - RC_channel_3.real_Ctr_ang);

        
        PPV2_U_input(trachea_init); 
        PPV6_U_input(trachea_init - trachea_1_6_bias);
        input_ppv_ccr_2 = trachea_init;
        input_ppv_ccr_6 = trachea_init;
    }

    else
    {
        // 1 6有偏差 早知道当时板子焊接多几路备用
        PPV1_U_input(trachea_init - trachea_1_6_bias); 
        PPV5_U_input(trachea_init); 
        input_ppv_ccr_1 = trachea_init;
        input_ppv_ccr_5 = trachea_init;

        
        PPV2_U_input(trachea_init); 
        PPV6_U_input(trachea_init - trachea_1_6_bias);
        input_ppv_ccr_2 = trachea_init;
        input_ppv_ccr_6 = trachea_init;

        // PPV1_U_input(trachea_zero_output); 
        // PPV5_U_input(trachea_zero_output); 

        // PPV2_U_input(trachea_zero_output); 
        // PPV6_U_input(trachea_zero_output); 
    }
}

//elbow  -2 到 - 30 
void ppv_elbow_control(void)
{
    // 肘部尝试
    if(tendon_Ctr_Flag)
    {
        //肘部PID控制                 
        elbow_last_angle_error = elbow_angle_error;
        elbow_angle_error = elbow_target_angle - elbow_angle; 
        elbow_PWM_Output_real += -elbow_kp * elbow_angle_error - elbow_kd * (elbow_angle_error - elbow_last_angle_error);
        // 数值分析                      一般都为负 除非超调                         最小：-28  最大：0
        elbow_PWM_Output = (int16_t)elbow_PWM_Output_real; //需要基本纯正的才行
        elbow_trachea_ = trachea_zero_output + elbow_PWM_Output;
        if(elbow_trachea_ >= elbow_trachea_max_limit) elbow_trachea_ = elbow_trachea_max_limit;
        else if(elbow_trachea_ <= trachea_zero_output + 100) elbow_trachea_ = trachea_zero_output + 100;

        
        // input_ppv_ccr_4 = elbow_trachea_;
        // input_ppv_ccr_7 = elbow_trachea_;
        // PPV7_U_input(elbow_trachea_);
        // PPV4_U_input(elbow_trachea_);

        // PPV7_U_input(elbow_trachea_max_limit); 
        // PPV4_U_input(elbow_trachea_max_limit); 
        // PPV7_U_input(elbow_trachea_min_limit); 
        // PPV4_U_input(elbow_trachea_min_limit); 
        
        elbow_PWM_Output_real = 0;
        input_ppv_ccr_4 = trachea_zero_output;
        input_ppv_ccr_7 = trachea_zero_output;
        PPV7_U_input(trachea_zero_output);   // roll -2.1左右
        PPV4_U_input(trachea_zero_output); 
    }

    else if(remote_ctr_mode == 1)
    {
        input_ppv_ccr_4 = RC_channel_7.real_Ctr_ang; 
        input_ppv_ccr_7 = RC_channel_7.real_Ctr_ang;

        PPV7_U_input(input_ppv_ccr_4); 
        PPV4_U_input(input_ppv_ccr_7); 
    }

    else
    {

        input_ppv_ccr_4 = trachea_zero_output; //对应roll -19°
        input_ppv_ccr_7 = trachea_zero_output;

        PPV7_U_input(input_ppv_ccr_4); 
        PPV4_U_input(input_ppv_ccr_7); 
    }
}



